"""
GomerX 机械臂控制模块
用于控制机器人机械臂的运动
"""

# 导入必要的系统模块
import time  # 用于时间相关功能，如延时
import math  # 用于数学计算
from config.settings import ARM_CONFIG  # 导入机械臂配置参数

class Arm:
    """GomerX 机械臂控制类
    
    负责控制机械臂的各种运动，包括移动到指定位置、抓取和放置物品等
    """
    
    def __init__(self):
        """初始化机械臂控制器
        
        从配置文件加载默认参数，并初始化机械臂状态
        """
        # 从配置文件加载机械臂的默认高度参数
        self.default_height = ARM_CONFIG["default_height"]  # 默认操作高度
        self.pick_height = ARM_CONFIG["pick_height"]  # 抓取物品时的高度
        self.place_height = ARM_CONFIG["place_height"]  # 放置物品时的高度
        
        # 加载速度和安全位置参数
        self.speed = ARM_CONFIG["speed"]  # 机械臂移动速度
        self.safe_position = ARM_CONFIG["safe_position"]  # 安全位置坐标
        
        # 初始化机械臂状态
        self.is_connected = False  # 机械臂连接状态，初始为未连接
        # 初始化当前位置为原点，高度为默认高度
        self.current_position = [0, 0, self.default_height]
        
    def connect(self):
        """连接到机械臂
        
        建立与机械臂硬件的通信连接
        
        Returns:
            bool: 连接是否成功
        """
        try:
            # 这里应该是连接到机械臂的实际代码
            # 由于没有真实的SDK，这里只是模拟连接过程
            self.is_connected = True  # 设置连接状态为已连接
            print("机械臂连接成功")  # 输出连接成功信息
            return True
        except Exception as e:
            # 捕获连接过程中的异常
            print(f"连接机械臂失败: {e}")  # 输出错误信息
            return False
    
    def disconnect(self):
        """断开与机械臂的连接
        
        关闭与机械臂硬件的通信连接
        """
        if self.is_connected:  # 检查是否已连接
            # 这里应该是断开连接的实际代码
            self.is_connected = False  # 设置连接状态为未连接
            print("机械臂连接已断开")  # 输出断开连接信息
    
    def move_to_position(self, x, y, z, speed=None):
        """
        移动机械臂到指定位置
        
        控制机械臂末端执行器移动到三维空间中的指定坐标
        
        Args:
            x: X坐标 (mm) - 前后方向
            y: Y坐标 (mm) - 左右方向
            z: Z坐标 (mm) - 上下方向
            speed: 移动速度，如果为None则使用默认速度
            
        Returns:
            bool: 移动是否成功
        """
        # 检查机械臂是否已连接
        if not self.is_connected:
            print("机械臂未连接")  # 输出错误信息
            return False  # 返回失败
            
        # 确定移动速度，使用传入的速度或默认速度
        move_speed = speed if speed is not None else self.speed
        
        try:
            # 这里应该是控制机械臂移动的实际代码
            # 输出移动信息，便于调试和观察
            print(f"机械臂移动到位置: ({x}, {y}, {z})，速度: {move_speed}")
            
            # 更新当前位置记录
            self.current_position = [x, y, z]
            
            # 模拟移动时间，实际应用中这里应该是等待机械臂完成移动的代码
            time.sleep(1.0)
            
            return True  # 返回成功
        except Exception as e:
            # 捕获移动过程中的异常
            print(f"控制机械臂移动失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def move_relative(self, dx, dy, dz, speed=None):
        """
        相对移动机械臂
        
        从当前位置相对移动指定的距离
        
        Args:
            dx: X方向相对移动距离 (mm) - 正值向前，负值向后
            dy: Y方向相对移动距离 (mm) - 正值向右，负值向左
            dz: Z方向相对移动距离 (mm) - 正值向上，负值向下
            speed: 移动速度，如果为None则使用默认速度
            
        Returns:
            bool: 移动是否成功
        """
        # 计算目标位置：当前位置 + 相对移动距离
        target_x = self.current_position[0] + dx
        target_y = self.current_position[1] + dy
        target_z = self.current_position[2] + dz
        
        # 调用绝对移动函数移动到目标位置
        return self.move_to_position(target_x, target_y, target_z, speed)
    
    def move_to_safe_position(self):
        """
        移动机械臂到安全位置
        
        将机械臂移动到预设的安全位置，通常用于初始化或避免碰撞
        
        Returns:
            bool: 移动是否成功
        """
        # 移动到预设的安全位置坐标
        return self.move_to_position(
            self.safe_position[0],  # 安全位置X坐标
            self.safe_position[1],  # 安全位置Y坐标
            self.safe_position[2]   # 安全位置Z坐标
        )
    
    def pick_object(self, x, y):
        """
        在指定位置抓取物品
        
        执行完整的抓取动作序列：移动到物品上方、下降、等待夹爪、上升
        
        Args:
            x: 物品X坐标 (mm)
            y: 物品Y坐标 (mm)
            
        Returns:
            bool: 抓取是否成功
        """
        # 检查机械臂是否已连接
        if not self.is_connected:
            print("机械臂未连接")  # 输出错误信息
            return False  # 返回失败
            
        try:
            # 1. 移动到物品上方安全高度
            print(f"移动到物品({x}, {y})上方安全高度")
            self.move_to_position(x, y, self.default_height)
            
            # 2. 下降到抓取高度
            print(f"下降到抓取高度: {self.pick_height}mm")
            self.move_to_position(x, y, self.pick_height)
            
            # 3. 等待夹爪完成抓取动作
            time.sleep(0.5)  # 给夹爪留出抓取时间
            
            # 4. 上升到安全高度，避免移动时碰撞其他物品
            print("上升到安全高度")
            self.move_to_position(x, y, self.default_height)
            
            return True  # 返回成功
        except Exception as e:
            # 捕获抓取过程中的异常
            print(f"抓取物品失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def place_object(self, x, y):
        """
        在指定位置放置物品
        
        执行完整的放置动作序列：移动到放置位置上方、下降、等待夹爪、上升
        
        Args:
            x: 放置位置X坐标 (mm)
            y: 放置位置Y坐标 (mm)
            
        Returns:
            bool: 放置是否成功
        """
        # 检查机械臂是否已连接
        if not self.is_connected:
            print("机械臂未连接")  # 输出错误信息
            return False  # 返回失败
            
        try:
            # 1. 移动到放置位置上方安全高度
            print(f"移动到放置位置({x}, {y})上方安全高度")
            self.move_to_position(x, y, self.default_height)
            
            # 2. 下降到放置高度
            print(f"下降到放置高度: {self.place_height}mm")
            self.move_to_position(x, y, self.place_height)
            
            # 3. 等待夹爪完成放置动作
            time.sleep(0.5)  # 给夹爪留出释放时间
            
            # 4. 上升到安全高度，避免移动时碰撞其他物品
            print("上升到安全高度")
            self.move_to_position(x, y, self.default_height)
            
            return True  # 返回成功
        except Exception as e:
            # 捕获放置过程中的异常
            print(f"放置物品失败: {e}")  # 输出错误信息
            return False  # 返回失败
    
    def get_current_position(self):
        """
        获取机械臂当前位置
        
        Returns:
            list: 当前位置坐标 [x, y, z]
        """
        # 返回当前位置的副本，避免外部修改内部状态
        return self.current_position.copy()
    
    def calculate_reachable_position(self, object_x, object_y, object_z=0):
        """
        计算机械臂可以抓取物品的位置
        
        根据机械臂的工作空间限制，计算是否可以到达指定物品位置，
        如果超出工作范围，则计算最接近的可到达位置
        
        Args:
            object_x: 物品X坐标 (mm)
            object_y: 物品Y坐标 (mm)
            object_z: 物品Z坐标 (mm)，默认为0
            
        Returns:
            tuple: (可到达的x, y, z坐标，是否可达)
        """
        # 这里应该根据机械臂的实际运动范围和工作空间计算
        # 由于没有具体的机械臂参数，这里简化处理
        
        # 假设机械臂的工作范围是半径200mm的圆柱体
        max_radius = 200  # 最大工作半径(mm)
        # 计算物品到原点的距离
        distance = math.sqrt(object_x**2 + object_y**2)
        
        if distance > max_radius:
            # 物品超出工作范围，计算最大可达位置
            # 计算缩放比例，使目标点位于工作范围边界上
            scale = max_radius / distance
            # 按比例缩放坐标
            reachable_x = object_x * scale
            reachable_y = object_y * scale
            reachable_z = self.pick_height  # 使用抓取高度
            reachable = False  # 标记为不可达
        else:
            # 物品在工作范围内，可以直接使用原坐标
            reachable_x = object_x
            reachable_y = object_y
            reachable_z = self.pick_height  # 使用抓取高度
            reachable = True  # 标记为可达
            
        # 返回计算结果
        return reachable_x, reachable_y, reachable_z, reachable